from cell import Cell
import math

"""
This class defines the map structure.
"""
class MapStructure:
    
    def __init__(self, rows, cols, cellWidth, cellHeight):
        self.rows = rows
        self.cols = cols
        self.cellWidth = cellWidth
        self.cellHeight = cellHeight
        # range
        
        #
        self.posXposY = [[0 for col in range(rows)] for row in range(cols)]
        self.posXnegY = [[0 for col in range(rows)] for row in range(cols)]   
        self.negXposY = [[0 for col in range(rows)] for row in range(cols)]
        self.negXnegY = [[0 for col in range(rows)] for row in range(cols)]
        #
        self.cellObstacles = []
        self.cellObstacleName = {}
        self.freeEdgeThreshold = 30
    
    def __scanLine__(self, x1, y1, x2, y2):        
        if x1 < x2:
            startX = x1
            startY = y1
            endX = x2
            endY = y2
        else:
            startX = x2
            startY = y2
            endX = x1
            endY = y1 
        curCellX, curCellY =  self.geoCord2CellCord(startX, startY)
        
        if startY > endY:
            offset = -1
        else:
            offset = 1
        
        if math.fabs(float(x2)-float(x1))/float(self.cellHeight) > \
            (math.fabs(float(x2)-float(x1))/self.cellHeight):
            stepx = int(math.fabs(x2-x1)/self.cellHeight) + 1
        else:
            stepx = int(math.fabs(x2-x1)/self.cellHeight)
        if math.fabs(float(y2)-float(y1))/float(self.cellHeight) > \
            (math.fabs(float(y2)-float(y1))/self.cellHeight):
            stepy = int(math.fabs(y2-y1)/self.cellWidth) + 1
        else: 
            stepy = int(math.fabs(y2-y1)/self.cellWidth)  
            
        if stepx > stepy:
            steps = stepx
            tan = (float(y2)-float(y1)) / (float(x2)-float(x1))
            for step in range(steps):
                tempy = (curCellX+1) * self.cellWidth * tan
                if tempy > ((curCellY+1)*self.cellHeight):
                    curCellY += offset
                #
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    return True
                curCellX += 1
        else:
            steps = stepy
            tan = (float(x2)-float(x1)) / (float(y2)-float(y1))               
            for step in range(steps):
                tempx = (curCellY+1) * self.cellHeight * tan
                if tempx > ((curCellX+1)*self.cellWidth):
                    curCellX += 1  
                #     
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    return True     
                curCellY += offset
        return False

    def __occupiedLine__(self, x, y, pos):
        curCellX = curCellY = 0  
        if float(x)/float(self.cellHeight) > (x/self.cellHeight):
            stepx = int(x/self.cellHeight) + 1
        else:
            stepx = int(x/self.cellHeight)
        if float(y)/float(self.cellHeight) > (y/self.cellWidth):
            stepy = int(y/self.cellWidth) + 1
        else: 
            stepy = int(y/self.cellWidth)
        if stepx > stepy:
            steps = stepx
            tan = float(y) / float(x)
            for step in range(steps):
                tempy = (curCellX+1) * self.cellWidth * tan
                if tempy > ((curCellY+1)*self.cellHeight):
                    curCellY += 1
                pos[curCellX][curCellY] = "free"
                curCellX += 1

        else:
            steps = stepy
            tan = float(x) / float(y)                
            for step in range(steps):
                tempx = (curCellY+1) * self.cellHeight * tan
                if tempx > ((curCellX+1)*self.cellWidth):
                    curCellX += 1                    
                pos[curCellX][curCellY] = "free"
                curCellY += 1
#                print "curx ", curCellX   
#                print "cury ", curCellY
                
    def insert(self, x, y, label):
        if x>0 and y>0:
            cellX = int(round(x / self.cellHeight))
            cellY = int(round(y / self.cellWidth))
#            print cellX
#            print cellY      
            self.posXposY[cellX][cellY] = label
        elif x>0 and y<0:
            cellX = int(round(x / self.cellHeight))
            cellY = int(round((-y) / self.cellWidth))         
            self.posXnegY[cellX][cellY] = label
            
        elif x<0 and y>0:
            cellX = int(round((-x) / self.cellHeight))
            cellY = int(round(y / self.cellWidth))
            self.negXposY[cellX][cellY] = label
        elif x<0 and y<0:
            cellX = int(round((-x) / self.cellHeight))
            cellY = int(round((-y) / self.cellWidth))            
            self.negXnegY[cellX][cellY] = label
        
    def clear(self):
        self.posXposY[:] = [[0 for col in range(self.rows)] for row in range(self.cols)]
        self.posXnegY[:] = [[0 for col in range(self.rows)] for row in range(self.cols)]
        self.negXposY[:] = [[0 for col in range(self.rows)] for row in range(self.cols)]
        self.negXnegY[:] = [[0 for col in range(self.rows)] for row in range(self.cols)]
        
    def occupy(self, x, y):
        cellX, cellY = self.geoCord2CellCord(x, y)
        cell = Cell(cellX, cellY, "obstacle")
        
        if self.getCellValue(cell) != "obstacle":
            self.cellObstacles.append(cell)
            self.cellObstacleName[(cell.xIndex, cell.yIndex)] = cell 
            self.setCellVaule(cell)
    
        if x>=0 and y>=0:
            self.__occupiedLine__(x, y, self.posXposY)
        elif x>=0 and y<=0:
            self.__occupiedLine__(x, -y, self.posXnegY)   
        elif x<=0 and y>=0:
            self.__occupiedLine__(-x, y, self.negXposY)
        elif x<=0 and y<=0:
            self.__occupiedLine__(-x, -y, self.negXnegY)
            
    def geoCord2CellCord(self, x, y):
        if float(x)/float(self.cellHeight) > (x/self.cellHeight) and float(x)/float(self.cellHeight) > 1:
            cellX = int(x/self.cellHeight) + 1
        else:
            cellX = int(x/self.cellHeight)
        if float(y)/float(self.cellHeight) > (y/self.cellWidth) and float(y)/float(self.cellHeight) > 1:
            cellY = int(y/self.cellWidth) + 1
        else: 
            cellY = int(y/self.cellWidth)
        return (cellX, cellY) 
    
    def getCellValue(self, cell):
        return self.getCellCoordinateValue(cell.xIndex, cell.yIndex)
    
    def setCellVaule(self, cell):
        self.setCellCoordinate(cell.xIndex, cell.yIndex, cell.value)
    
    def updateCellOrNot(self, cell):
#        if self.getCellValue(cell) == cell.value:
        if self.getCellValue(cell) == "free" or self.getCellValue(cell) == 0:
            return True
        else:
            return False
    
    def getRows(self):
        return self.rows
    
    def getCols(self):
        return self.cols
    
    """
    
    """
    def setGeoCoordinate(self, x, y, value):
        cellX, cellY = self.geoCord2CellCord(x, y)
        self.setCellCoordinate(cellX, cellY, value)
        
    """
    
    """
    def setCellCoordinate(self, xIndex, yIndex, value):
        if xIndex>=0 and yIndex>=0:
            self.posXposY[xIndex][yIndex] = value    
        elif xIndex>=0 and yIndex<=0:
            self.posXnegY[xIndex][-yIndex] = value
        elif xIndex<=0 and yIndex>=0:
            self.negXposY[-xIndex][yIndex] = value
        elif xIndex<=0 and yIndex<=0:
            self.negXnegY[-xIndex][-yIndex] = value     
        
    """
    
    """
    def getGeoCoordinateValue(self, x, y):
        cellX, cellY = self.geoCord2CellCord(x, y)
        return self.getCellCoordinateValue(cellX, cellY)
   
    """
    
    """
    def getCellCoordinateValue(self, xIndex, yIndex):
        if xIndex>=0 and yIndex>=0:
            return self.posXposY[xIndex][yIndex]    
        elif xIndex>=0 and yIndex<=0:
            return self.posXnegY[xIndex][-yIndex]
        elif xIndex<=0 and yIndex>=0:
            return self.negXposY[-xIndex][yIndex]
        elif xIndex<=0 and yIndex<=0:
            return self.negXnegY[-xIndex][-yIndex] 
        
    
    """
    return [[obstacle1, obstacle2], ..., [obstacleX, obstacleY]]
    """
    def getFreeEdgeList(self):
        freeEdgeList = []
        
        self.cellObstacles.sort()        
#        freeEdgeList = self.cellObstacles
        
        prevObstacle = None
        first = True
        for obstacle in self.cellObstacles:
            if first == True:
                prevObstacle = obstacle
                first = False
                continue
            else:
                if prevObstacle.distance(obstacle) > self.freeEdgeThreshold:
                    freeEdgeSubSet = []
                    freeEdgeSubSet.append(prevObstacle)
                    freeEdgeSubSet.append(obstacle)    
                    freeEdgeList.append(freeEdgeSubSet)  
                          
                prevObstacle = obstacle

        return freeEdgeList
    
    """
    return the middle point geography coordinate of a cell
    """
    def getCellCoordinate(self, cell):
        return ((cell.xIndex)*self.cellHeight+self.cellHeight/2, 
                (cell.yIndex)*self.cellWidth+self.cellWidth/2)
    
    """
    
    """
    def hitObstacleOrNot(self, x1, y1, x2, y2):
        if x1 < x2:
            curCellX, curCellY =  self.geoCord2CellCord(x1, y1)
            if y1 > y2:
                offset = -1
            else:
                offset = 1
        else:
            curCellX, curCellY =  self.geoCord2CellCord(x2, y2)
            if y2 > y1:
                offset = -1
            else:
                offset = 1
        
        if math.fabs(float(x2)-float(x1))/float(self.cellHeight) > \
            (math.fabs(float(x2)-float(x1))/self.cellHeight):
            stepx = int(math.fabs(x2-x1)/self.cellHeight) + 1
        else:
            stepx = int(math.fabs(x2-x1)/self.cellHeight)
        if math.fabs(float(y2)-float(y1))/float(self.cellHeight) > \
            (math.fabs(float(y2)-float(y1))/self.cellHeight):
            stepy = int(math.fabs(y2-y1)/self.cellWidth) + 1
        else: 
            stepy = int(math.fabs(y2-y1)/self.cellWidth)  
            
        if stepx > stepy:
            steps = stepx
            tan = (float(y2)-float(y1)) / (float(x2)-float(x1))
            for step in range(steps):
                tempy = (curCellX+1) * self.cellWidth * tan
                if tempy > ((curCellY+1)*self.cellHeight):
                    curCellY += offset
                #
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    return True
                curCellX += 1
        else:
            steps = stepy
            tan = (float(x2)-float(x1)) / (float(y2)-float(y1))               
            for step in range(steps):
                tempx = (curCellY+1) * self.cellHeight * tan
                if tempx > ((curCellX+1)*self.cellWidth):
                    curCellX += 1  
                #     
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    return True     
                curCellY += offset
        return False
    
    def hitWhichObstacle(self, x1, y1, x2, y2):
        if x1 < x2:
            curCellX, curCellY =  self.geoCord2CellCord(x1, y1)
            if y1 > y2:
                offset = -1
            else:
                offset = 1
        else:
            curCellX, curCellY =  self.geoCord2CellCord(x2, y2)
            if y2 > y1:
                offset = -1
            else:
                offset = 1
        
        if math.fabs(float(x2)-float(x1))/float(self.cellHeight) > \
            (math.fabs(float(x2)-float(x1))/self.cellHeight):
            stepx = int(math.fabs(x2-x1)/self.cellHeight) + 1
        else:
            stepx = int(math.fabs(x2-x1)/self.cellHeight)
        if math.fabs(float(y2)-float(y1))/float(self.cellHeight) > \
            (math.fabs(float(y2)-float(y1))/self.cellHeight):
            stepy = int(math.fabs(y2-y1)/self.cellWidth) + 1
        else: 
            stepy = int(math.fabs(y2-y1)/self.cellWidth)  
            
        if stepx > stepy:
            steps = stepx
            tan = (float(y2)-float(y1)) / (float(x2)-float(x1))
            for step in range(steps):
                tempy = (curCellX+1) * self.cellWidth * tan
                if tempy > ((curCellY+1)*self.cellHeight):
                    curCellY += offset
                #
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    cell = Cell(curCellX, curCellY)
                    x, y = self.getCellCoordinate(cell)
                    point = Point(x, y)
                    return point
                curCellX += 1
        else:
            steps = stepy
            tan = (float(x2)-float(x1)) / (float(y2)-float(y1))               
            for step in range(steps):
                tempx = (curCellY+1) * self.cellHeight * tan
                if tempx > ((curCellX+1)*self.cellWidth):
                    curCellX += 1  
                #     
                if self.cellObstacleName.__contains__((curCellX, curCellY)):
                    cell = Cell(curCellX, curCellY)
                    x, y = self.getCellCoordinate(cell)
                    point = Point(x, y)
                    return point     
                curCellY += offset
        # return an invalid value
        return Point(100000, 100000)
    
    def getCurrentXYRange(self):
        self.cellObstacles.sort()
        
        maxX = maxY = -1000000
        minX = minY = 1000000
        for obstacle in self.cellObstacles:
            if obstacle.xIndex > maxX:
                maxX = obstacle.xIndex
            if obstacle.xIndex < minX:
                minX = obstacle.xIndex
            if obstacle.yIndex > maxY:
                maxY = obstacle.yIndex
            if obstacle.yIndex < minY:
                minY = obstacle.yIndex
        return minX*self.cellHeight, maxX*self.cellHeight, minY*self.cellWidth, maxY*self.cellWidth
    
    def addObstacle(self, cell):
        if not self.cellObstacleName.__contains__((cell.xIndex, cell.yIndex)):
            self.cellObstacles.append(cell)
            self.cellObstacleName[(cell.xIndex, cell.yIndex)] = cell         

    """
    return True/False
    """
    def isFreeEdge(self, x1, y1, x2, y2):               
        midx = float((x1+x2)/2.0)
        midy = float((y1+y2)/2.0)
        
        print "x1 ", x1
        print "y1 ", y1
        print "x2 ", x2
        print "y2 ", y2
        print "midx ", midx
        print "midy ", midy
        
#        midValue = self.getGeoCoordinateValue(midValue)
#        if midValue != 0 :
#            return False
#        else: 
#            return True
        
        range = 100
        upValue = self.getGeoCoordinateValue(midx + range, midy)
        downValue = self.getGeoCoordinateValue(midx - range, midy)
        leftValue = self.getGeoCoordinateValue(midx, midy + range)
        rightValue = self.getGeoCoordinateValue(midx, midy - range) 
        
        
        print "upValue ", upValue
        print "downValue ", downValue
        print "leftValue ", leftValue
        print "rightValue ", rightValue
        
        
        if upValue != 0 and downValue != 0 and leftValue != 0 \
           and rightValue != 0:
            return False
        else:
            return True

if __name__=="__main__":        
    map = MapStructure(20, 20, 2, 2)
    map.occupy(1,11)
    
    print map.posXposY[0][5]
    print map.posXposY[0][6]
    
    print "***************cell cord***************"
    map.setCellCoordinate(2, -1, "obstacle")
    print map.getCellCoordinateValue(-1, -1)
    print map.getCellCoordinateValue(2, -1)
    
    print "***************geo cord***************"
#    map.setGeoCoordinate(1.5, 2.5, "obstacle")
    print map.getGeoCoordinateValue(1.5, 2.5)
    print map.getGeoCoordinateValue(3, 2.5)
    print map.getGeoCoordinateValue(1, 11)
    
    print "***************object compare***************"
    cellList = [Cell(3, 4, 0), Cell(3, -5, 0), Cell(-2, -4, 0), Cell(-2, 4, 0), Cell(4, -7, 0)]
    cellList.sort()
    for cell in cellList:
        print cell
        
    print "***************free edge***************"
    testMap = MapStructure(500, 500, 50, 50)
#    cellGeoList = [(0.10000000000002027, 331.03191481533935), (57.09765613651517, 323.21377614415354), (130.37775451694995, 357.8928653732105), (219.58083575099823, 380.13473448976515), (336.98665170318429, 401.47670210504475), (476.29539846295393, 399.560984510075), (2620.5898121972414, 1512.9183296971), (4129.7288344697708, 1503.0484780351428), (5305.4775700413584, 935.47516757121673), (5267.7793434375453, 0.0), (2674.3126468077398, -471.52980091141285), (1827.3408797563434, -665.04779478641478), (1603.9176408045291, -925.94237786591646), (1150.7191532581101, -965.46970841588677), (709.99472755428769, -846.01041656725067), (527.40632163398357, -913.3041159036718), (334.08533072902594, -917.57483118772052), (245.39576918764712, -1391.1054413593579), (0.10000000000007872, -1285.6523190749233)]
#    cellGeoList = [(1898.7031191033084, -538.3614174349492), (1656.6481486271864, -801.9089043514208), (1065.7733933260974, -769.40346936544734), (790.99881434641804, -814.10244700453666), (546.03706447344916, -804.14902980609872), (482.04685749896197, -1073.8503407089431), (325.85459503375375, -1290.2267259680211), (96.079585589521017, -1316.559653694251), (-130.15300481126224, -1275.5764699045649), (-350.49553409006774, -1236.4606708095348), (-606.0566837119128, -1252.1658402660685), (-957.73670734175266, -1326.5249425392146), (-1349.2371721713023, -1310.6994731535126), (-1379.3017237499948, -936.33308064319397), (-1398.2680832261462, -627.55290278571886), (-1284.5189682508374, -324.35318755375658), (-1270.853303821973, -92.686977369274004), (-1316.7563490834914, 134.43665348948593), (-1267.3459434857666, 359.33446821347923)]
#    cellGeoList = [(-1315.4952815513855, -741.25809329288677), (-1342.7495411769694, -472.85783528619311), (-1273.7364571826727, -210.84446074631759), (-1297.1262694166874, 13.616169248456918), (-1354.0470084509118, 253.44268912839786), (-933.68835055451405, 351.02831956781381), (-535.15175097668123, 316.61537992799913), (-388.99344636042417, 333.58065796624578), (-263.04222374435102, 320.47253497383872), (-163.24692456846705, 290.06378940175438), (-101.84861787706873, 289.7666441778386), (-47.729851351247369, 289.50164260609108), (3.1906855972389279, 285.2361532406785), (53.270998112561728, 283.55616471704849), (102.13959506912592, 271.1592132490569), (169.75858823203242, 286.65359187530817), (232.04807930591565, 270.42889041670469), (384.98334426526043, 316.00504255174343), (555.78289273592861, 312.99250054416103)]
    cellGeoList = [(0.10000000000001918, 313.45326557372221), (30.213336940553035, 344.16574032589824), (61.836653190072312, 350.08996397721404), (96.456793417861334, 359.56876602247951), (125.9761701319719, 345.7996116656592), (152.88029910011048, 327.6472094513677), (216.65922702565564, 375.07435973763523), (262.31807481627987, 374.4741171900011), (331.73011538924601, 395.21220606830695), (391.80056137683613, 391.68931137683609), (506.82065838028404, 425.17471884829729), (592.45489678967522, 414.75321438240275), (753.34756397258093, 434.86551504235069), (955.32694182235025, 445.41234979047186), (1274.044136302311, 463.6642494527236), (1262.1860327621807, 338.16543960817188), (1314.3953016408771, 231.73968142506851), (1304.0441873296065, 114.07739817892362), (1280.8424174413267, 0.0), (1285.5081694910716, -112.45570675113751), (1265.6471205968658, -223.14406184654837), (1259.9273992493054, -337.5602405823991), (1250.7411348414389, -455.18265055190443), (2221.3643484303439, -1035.7752880024732), (2315.6558065586837, -1336.8645994566079), (2168.6047079544278, -1518.3851935077728), (2275.1472696259634, -1908.976926145516), (2276.0706550360869, -2275.9594050360861), (2300.447125068004, -2741.4377751542966), (2264.2457548809052, -3233.5231431161601), (2396.8011936913217, -4151.191013715702), (2008.7097315974045, -4307.4862707271413), (1542.3816793095623, -4237.3417649222138), (1189.2520956978037, -4437.9363564038149), (791.2111785952917, -4486.5784496001288), (367.23974095004922, -4196.3951655660167), (0.1000000000001065, -1739.4256911694886)]
#    cellGeoList += [(0.10000000000002027, 331.03191481533935), (57.09765613651517, 323.21377614415354), (130.37775451694995, 357.8928653732105), (219.58083575099823, 380.13473448976515), (336.98665170318429, 401.47670210504475), (476.29539846295393, 399.560984510075), (2620.5898121972414, 1512.9183296971), (4129.7288344697708, 1503.0484780351428), (5305.4775700413584, 935.47516757121673), (5267.7793434375453, 0.0), (2674.3126468077398, -471.52980091141285), (1827.3408797563434, -665.04779478641478), (1603.9176408045291, -925.94237786591646), (1150.7191532581101, -965.46970841588677), (709.99472755428769, -846.01041656725067), (527.40632163398357, -913.3041159036718), (334.08533072902594, -917.57483118772052), (245.39576918764712, -1391.1054413593579), (0.10000000000007872, -1285.6523190749233)]
    
    print "cellGeoList.__len__() ", cellGeoList.__len__()
    
    for cellGeo in cellGeoList:
#        print "cellGeo[0] ", cellGeo[0]
#        print "cellGeo[1] ", cellGeo[1]
#        testMap.occupy(cellGeo[0], cellGeo[1])
        cellXIndex, cellYIndex = testMap.geoCord2CellCord(cellGeo[0], cellGeo[1])
        cell = Cell(cellXIndex, cellYIndex, "obstacle")
        testMap.addObstacle(cell)
    
    print "current X Y range"
    minX, maxX, minY, maxY = testMap.getCurrentXYRange()
    
    print "obstacle sorted list"
    print "testMap.cellObstacles.__len__() ", testMap.cellObstacles.__len__()
    for obstacle in testMap.cellObstacles:
        print obstacle
        
    print "free edge"
    freeEdges = testMap.getFreeEdgeList()
    print "freeEdges.__len__() ", freeEdges.__len__()
    for freeEdge in freeEdges:
        print freeEdge[0]
        print freeEdge[1]
        print "*********"
    
    print "***************hit obstacle***************"    
    obstacleMap = MapStructure(20, 20, 2, 2)
    obstacleMap.occupy(1, 11)
    
    print obstacleMap.hitObstacleOrNot(2, 0, float(-1/11), 13)

#    print "***************range test***************"
#    rangeMap = MapStructure(500, 500, 50, 50)
#    rangeMap.getGeoCoordinateValue(x, y)
    
    
    print "*************************************************"
    
    list1 = [[(1627.451744993886, -436.03809222343614), (0.10000000000014062, -2296.5965610597918)]]
    print list1
    
    list2 = [[(33, 45), (22, 12)]]
    list3 = list1 + list2
    
    print list3
    
#    del list[0]
#    
#    print list
#    
#    for element in list:
#        print element
#        print element[0][0]
        
    
    
    
